﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using Microsoft.Ccr.Core;
using sicklrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using cs = Microsoft.Dss.Services.Constructor;
using Microsoft.Dss.ServiceModel.Dssp;
using System.Drawing.Drawing2D;
using System.IO;
using Microsoft.Dss.Core;
using Microsoft.Robotics.Simulation.Physics.Proxy;
using Microsoft.Robotics.PhysicalModel.Proxy;

namespace SimPioneersControl
{
    public partial class VFHForm : Form
    {
        List<int> X = new List<int>();
        int s = 0;
        double bestSector = -1;

        public VFHForm()
        {
            InitializeComponent();
        }

        /// <summary>
        /// Sets the images from the laser range measurements to the Cylinder and Top Views.
        /// Also it calls for the next method to evaluate the candidates and free sectors.
        /// </summary>
        /// <param name="stateType"></param>
        /// <param name="vfh"></param>
        public void LrfToImage(sicklrf.State stateType, VFH vfh)
        {
            #region Cylinder
            int width = stateType.DistanceMeasurements.Length;
            int height = 100;

            Bitmap bmp = new Bitmap(width, height);
            Graphics g = Graphics.FromImage(bmp);
            g.Clear(Color.DimGray);
            int half = bmp.Height / 2;

            for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
            {
                int range = stateType.DistanceMeasurements[x];
                if (range > 0 && range < 8192)
                {
                    int h = bmp.Height * 500 / stateType.DistanceMeasurements[x];
                    if (h < 0)
                    {
                        h = 0;
                    }
                    Color col = LinearColor(Color.Red, Color.LightGray, 0, 8192, range);
                    g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
                }
            }
            LRFCyl.Image = bmp;
            #endregion

            #region Top
            //Sets the Top View
            int width2 = 630;
            int height2 = 300;
            Bitmap bmp2 = new Bitmap(width2, height2);
            Graphics g2 = Graphics.FromImage(bmp2);
            g2.Clear(Color.Blue);
            double angularOffset = -90 + stateType.AngularRange / 2.0;
            double piBy180 = Math.PI / 180.0;
            double halfAngle = stateType.AngularResolution / 2.0;
            double scale = height2 / 5000.0;

            GraphicsPath path = new GraphicsPath();
            for (int pass = 0; pass != 2; pass++)
            {
                for (int i = 0; i < stateType.DistanceMeasurements.Length; i++)
                {
                    int range = stateType.DistanceMeasurements[i];
                    if (range > 0 && range < 8192)
                    {
                        double angle = i * stateType.AngularResolution - angularOffset;
                        double lowAngle = (angle - halfAngle) * piBy180;
                        double highAngle = (angle + halfAngle) * piBy180;

                        double drange = range * scale;

                        float lx = (float)(height2 + drange * Math.Cos(lowAngle));
                        float ly = (float)(height2 - drange * Math.Sin(lowAngle));
                        float hx = (float)(height2 + drange * Math.Cos(highAngle));
                        float hy = (float)(height2 - drange * Math.Sin(highAngle));

                        if (pass == 0)
                        {
                            if (i == 0)
                            {
                                path.AddLine(height2, height2, lx, ly);
                            }
                            path.AddLine(lx, ly, hx, hy);
                        }
                        else
                        {
                            g2.DrawLine(Pens.DarkBlue, lx, ly, hx, hy);
                        }
                    }
                }

                if (pass == 0)
                {
                    g2.FillPath(Brushes.White, path);
                }
            }

            float botWidth = (float)(190 * scale);
            g2.DrawLine(Pens.Red, height2, height2 - botWidth, height2, height2);
            g2.DrawLine(Pens.Red, height2 - 3, height2 - botWidth, height2 + 3, height2 - botWidth);
            g2.DrawLine(Pens.Red, height2 - botWidth, height2 - 3, height - botWidth, height2);
            g2.DrawLine(Pens.Red, height2 + botWidth, height2 - 3, height + botWidth, height2);
            g2.DrawLine(Pens.Red, height2 - botWidth, height2 - 1, height + botWidth, height2 - 1);

            LRFTop.Image = bmp2;

            #endregion

            PolarToImage(stateType, vfh);
        }
        /// <summary>
        /// Plots the Polar histogram according to the laser measurements. The max value is normalized to 6000 mm.
        /// </summary>
        /// <param name="stateType"></param>
        /// <param name="vfh"></param>
        public void PolarToImage(sicklrf.State stateType, VFH vfh)
        {
            Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, 100);
            Graphics g = Graphics.FromImage(bmp);
            g.Clear(Color.White);
            int half = bmp.Height / 2;
            int h = 59;
            int norm_fact = 6000; //Max must be the largest LRF reading, which is 8192

            for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
            {
                //Normalize measurement 
                int norm = h * stateType.DistanceMeasurements[x] / norm_fact;
                //Plot line
                if (x % 3 == 0)
                    g.DrawLine(new Pen(Color.Blue), bmp.Width - x, bmp.Height - norm, bmp.Width - x, bmp.Height);
                else
                    g.DrawLine(new Pen(Color.DarkBlue), bmp.Width - x, bmp.Height - norm, bmp.Width - x, bmp.Height);
            }
            //Plot Threshold
            g.DrawLine(new Pen(Color.Lime), bmp.Width - 360, bmp.Height - (vfh.Thresh * h / norm_fact), bmp.Width, bmp.Height - (vfh.Thresh * h / norm_fact));


            //Plot Labels
            g.DrawString("180", new Font(FontFamily.GenericSansSerif, 14, GraphicsUnit.Pixel), Brushes.Gray, 0, 0);
            g.DrawString("0", new Font(FontFamily.GenericSansSerif, 14, GraphicsUnit.Pixel), Brushes.Gray, bmp.Width - 15, 0);
            g.DrawString("Th (mm): " + vfh.Thresh, new Font(FontFamily.GenericSansSerif, 14, GraphicsUnit.Pixel), Brushes.Gray, bmp.Width / 2 - 45, 0);

            LRFHist.Image = bmp;
            OptimalToImage(stateType, vfh);
        }
        /// <summary>
        /// Displays the Optimal Sector and the Candidate Sector on the form
        /// </summary>
        /// <param name="stateType"></param>
        /// <param name="vfh"></param>
        public void OptimalToImage(sicklrf.State stateType, VFH vfh)
        {
            Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, 100);
            Graphics g = Graphics.FromImage(bmp);
            g.Clear(Color.Red);
            int half = bmp.Height / 2;
            int h = 59;
            int counter = 0;
            X.Clear();

            for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
            {
                //Count consecutive measurements over the threshold
                if (stateType.DistanceMeasurements[x] >= vfh.Thresh)
                    counter++;
                //Reset counter if the current measurement[x] is below the threshold
                else
                    counter = 0;

                //Draw the free sectors
                Color col = Color.White;
                g.DrawRectangle(new Pen(col), bmp.Width - x, half - h, counter, 2 * h);

                //Draw eligible candidates
                if (counter >= vfh.Min4Candid && counter % vfh.Min4Candid == 0)
                {
                    //It's easier to manage a set of lines instead of rectangles, defined three just for better width.
                    Color col1 = Color.Blue;
                    g.DrawLine(new Pen(col1), bmp.Width - x + vfh.Min4Candid / 2, half - h, bmp.Width - x + vfh.Min4Candid / 2, half + h);
                    g.DrawLine(new Pen(col1), bmp.Width - x + 1 + vfh.Min4Candid / 2, half - h, bmp.Width - x + 1 + vfh.Min4Candid / 2, half + h);
                    g.DrawLine(new Pen(col1), bmp.Width - x - 1 + vfh.Min4Candid / 2, half - h, bmp.Width - x - 1 + vfh.Min4Candid / 2, half + h);
                    X.Add(x - vfh.Min4Candid / 2);
                }

            }

            s = vfh.Min4Candid;

            if (bestSector != -1)
            {
                Color col1 = Color.Lime;
                for (int i = 0; i < 4; i++)
                {
                    g.DrawLine(new Pen(col1), bmp.Width - (int)bestSector + i, bmp.Height, bmp.Width - (int)bestSector + i, 0);
                    g.DrawLine(new Pen(col1), bmp.Width - (int)bestSector - i, bmp.Height, bmp.Width - (int)bestSector - i, 0);
                }
            }

            LRFOptim.Image = bmp;
        }

        #region Helpers
        /// <summary>
        /// For making a linear scale of color gradients according to present value and a given range with the associated pair of colors.
        /// </summary>
        /// <param name="nearColor"></param>
        /// <param name="farColor"></param>
        /// <param name="nearLimit"></param>
        /// <param name="farLimit"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        private Color LinearColor(Color nearColor, Color farColor, int nearLimit, int farLimit, int value)
        {
            if (value <= nearLimit)
            {
                return nearColor;
            }
            else if (value >= farLimit)
            {
                return farColor;
            }

            int span = farLimit - nearLimit;
            int pos = value - nearLimit;

            int r = (nearColor.R * (span - pos) + farColor.R * pos) / span;
            int g = (nearColor.G * (span - pos) + farColor.G * pos) / span;
            int b = (nearColor.B * (span - pos) + farColor.B * pos) / span;

            return Color.FromArgb(r, g, b);
        }
        /// <summary>
        /// Calculates the best sector for turning in the corresponding units and range according to what the controller is expecting as alpha.
        /// </summary>
        /// <param name="angle2goal">Angle towards the given goal in radians</param>
        /// <param name="stateType">Sicklrf state containing the current measurements for scaling max speed</param>
        /// <returns></returns>
        public Tuple<double, double> CalcAngle(double angle2goal, sicklrf.State stateType)
        {

            Tuple<double, double> response = new Tuple<double, double>();

            response.Item0 = stateType.DistanceMeasurements[180];

            #region Calculate response.Item1 (Theta)
            //Choose the angle for the correct sector selection
            angle2goal = angle2goal * 180 / Math.PI; // Change to the range [-179.99,179.99]


            if (X.Count != 0)
            {
                int index = 0;
                if (angle2goal >= -90 && angle2goal <= 90)
                {
                    double minDif = Math.Abs(angle2goal + 90 - X[0] / 2.0);
                    for (int i = 1; i < X.Count; i++)
                    {
                        double Dif = Math.Abs(angle2goal + 90 - X[i] / 2.0);
                        if (Dif < minDif)
                            index = i;
                    }
                }
                else if ((angle2goal > -180 && angle2goal < -90) || (angle2goal > 90 && angle2goal < 180))
                {
                    response.Item1 = angle2goal * Math.PI / 180;
                    bestSector = -1;
                    return response;
                }


                //Index refinement
                response.Item1 = X[index];

                //if (index - 1 > 0 && index + 1 < X.Count - 1)
                //{
                //    if ((angle2goal + 90 - Math.Abs(X[index - 1] / 2.0) < Math.Abs(angle2goal + 90 - X[index + 1] / 2.0)) && Math.Abs(X[index - 1] - X[index]) <= s)
                //    {
                //        response.Item1 = X[index] - Math.Abs(X[index - 1] - X[index]) / 2;
                //    }
                //    else if ((angle2goal + 90 - Math.Abs(X[index + 1] / 2.0) <= Math.Abs(angle2goal + 90 - X[index - 1] / 2.0)) && Math.Abs(X[index + 1] - X[index]) <= s)
                //    {
                //        response.Item1 = X[index] + Math.Abs(X[index + 1] - X[index]) / 2;
                //    }
                //}

                //Send it for the optimal drawing just before making the adjustments for the control input.
                bestSector = response.Item1;
                //Assign the sector index to the angle: since sectors are from 0-360 but really imply 0-180 degrees, we divide it by 2.
                response.Item1 /= 2;
                //Adjust the output into the real angle data in degrees: if we are between 0-90 we mean a turn right, if between 90 and 180 it's a left turn.
                response.Item1 = response.Item1 - 90;
                //Finally, Set it to radians
                response.Item1 = response.Item1 * Math.PI / 180;
            }


            #endregion

            return response;
        }
        #endregion


    }
}
